#include "Vector3SIMD.h"

//intrinsics function
#include <mmintrin.h>

//malloc and free
#include <malloc.h>

#include <math.h>

//indexes
#define X 0
#define Y 1
#define Z 2
#define W 3

//constructor. Puts everything to 0.
Vector3SIMD::Vector3SIMD()
{
	_vector = _mm_setzero_ps();
	_vector.m128_f32[W] = 1;
}

//Constructor. Puts everything to the value of f.
Vector3SIMD::Vector3SIMD(float f)
{
	_vector = _mm_set1_ps(f);
	_vector.m128_f32[W] = 1;
}

//Constructor. Set the value to x, y and z.
Vector3SIMD::Vector3SIMD(float x, float y, float z)
{
	_vector.m128_f32[X] = x;
	_vector.m128_f32[Y] = y;
	_vector.m128_f32[Z] = z;
	_vector.m128_f32[W] = 1;
}

//Destructor.
Vector3SIMD::~Vector3SIMD()
{}

inline float Vector3SIMD::getX() const
{
	return _vector.m128_f32[0];
}

inline float Vector3SIMD::getY() const
{
	return _vector.m128_f32[1];
}

inline float Vector3SIMD::getZ() const
{
	return _vector.m128_f32[2];
}

inline float Vector3SIMD::getW() const
{
	return _vector.m128_f32[3];
}

void* Vector3SIMD::operator new(size_t size)
{
	return _aligned_malloc(size, 16);
}

void* Vector3SIMD::operator new[](size_t size)
{
	return _aligned_malloc(size, 16);
}

void Vector3SIMD::operator delete(void* mem)
{
	_aligned_free(mem);
}

void Vector3SIMD::operator delete[](void* mem)
{
	_aligned_free(mem);
}

inline Vector3SIMD::operator __m128 () const
{
	return _vector;
}

inline void Vector3SIMD::set(__m128 v)
{
	_vector = v;
}

inline void Vector3SIMD::set(float x, float y, float z)
{
	_vector.m128_f32[X] = x;
	_vector.m128_f32[Y] = y;
	_vector.m128_f32[Z] = z;
	_vector.m128_f32[W] = 1;
}

//norme
inline float Vector3SIMD::getNorme() const
{
	return sqrt(Vector3SIMDDot(*this, *this));
}

void Vector3SIMDAdd(const Vector3SIMD& v1, const Vector3SIMD& v2, Vector3SIMD& r)
{
	r.set(_mm_add_ps(v1, v2));
}

void Vector3SIMDSub(const Vector3SIMD& v1, const Vector3SIMD& v2, Vector3SIMD& r)
{
	r.set(_mm_sub_ps(v1, v2));
}

void Vector3SIMDMulF(const Vector3SIMD& v1, float f, Vector3SIMD& r)
{
	Vector3SIMD v2(f);
	r.set(_mm_mul_ps(v1, v2));
}

//multiplication
void Vector3SIMDMul(const Vector3SIMD& v1, const Vector3SIMD& v2, Vector3SIMD& r)
{
	r.set(_mm_mul_ps(v1, v2));
}

//dot
float Vector3SIMDDot(const Vector3SIMD& v1, const Vector3SIMD& v2)
{
	__m128 t = _mm_mul_ps(v1, v2);
	return t.m128_f32[X] + t.m128_f32[Y] + t.m128_f32[Z];
}

//normalize
void Vector3SIMDNormalize(const Vector3SIMD& v1, Vector3SIMD& out)
{
	float n = v1.getNorme();
	Vector3SIMDMulF(v1, 1/n, out);
}

//cross
void Vector3SIMDCross(const Vector3SIMD& v1, const Vector3SIMD& v2, Vector3SIMD& r)
{
	//shuffle and calculate the left part of the substraction
	__m128 sh1 = _mm_shuffle_ps(v1, v1, _MM_SHUFFLE(W, X, Z, Y));
	__m128 sh2 = _mm_shuffle_ps(v2, v2, _MM_SHUFFLE(W, Y, X, Z));
	__m128 r1 = _mm_mul_ps(sh1, sh2);

	//shuffle and calculate the right part of the substraction
	sh1 = _mm_shuffle_ps(v1, v1, _MM_SHUFFLE(W, Y, X, Z));
	sh2 = _mm_shuffle_ps(v2, v2, _MM_SHUFFLE(W, X, Z, Y));
	__m128 r2 = _mm_mul_ps(sh1, sh2);

	//substract
	r.set(_mm_sub_ps(r1, r2));
}